import cv2
from cv_bridge import CvBridge

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Image
from carsource_interfaces.msg import CameraOriInterface
 


FPS = float(30.0)

class PubImageNode(Node):
    def __init__(self):
        super().__init__("pubImage")

        self.construct_cameras( )
        self.br = CvBridge()
        self.camera_publishers = self.create_publisher(CameraOriInterface, "camera_ori_data", 10)         
        #定时器发布图片
        self.create_timer(1/FPS, self.cams)

    def construct_cameras(self):
        self.cameras =cv2.VideoCapture("/dev/video0")
               

    def cams(self):
        ret, frame = self.cameras.read()
        msg = CameraOriInterface()
        msg.explain = "ros2 : Image topic"  # string
        msg.image = self.br.cv2_to_imgmsg(frame) # Sensor_msg::Image
        if(ret):
            self.camera_publishers.publish(msg)
    


def main():
    rclpy.init()
    rclpy.spin(PubImageNode())
    rclpy.shutdown()


if __name__ == '__main__':
    main()
